#include "Viewer.h"

#include <QMouseEvent>
#include <QFileInfo>
#include <limits>

#include "glPrimitives.h"
#include "DeformableRegistration.h"

#define ARMA_DONT_USE_BLAS
#include <armadillo>

const double draw_handles_ratio = 0.03;
using namespace std;

Viewer::Viewer(QWidget *parent) : QGLViewer(parent)
	, btn_pressed(Qt::NoButton)
	, onDrag(false)
	, onEmbededDeformation(false), onHaoli(false), onGeodesic(false)
	, shader_program(NULL), VertexShader(NULL), FragmentShader(NULL)
{
	
	for (int i = 0; i<6; ++i)
		alph[i] = 0;
	LoadShader("directional.vert", "directional.frag");
// 	shader.GlewInit();
// 	shader.SetShaders("directional.vert", "directional.frag");
}

void Viewer::Clear()
{
	alph[0] = 50;			// e_rigid
	alph[1] = 100;			// e_smooth
	alph[2] = 0.000005;		// sigma
	alph[3] = 0.1;			// e_rigid is halved until this value

	alph[4] = DIS_THRESHOLD;
	alph[5] = ANG_THRESHOLD;

	onEmbededDeformation = false;
	onHaoli = false;
	onGeodesic = false;

	templ.Clear();
	templ_deformed.Clear();
	target.Clear();
	graph.Clear();

	handles.clear();
	correnpondence.clear();

	k_nearest_idx.clear();
	weight_value.clear();

	updateGL();	
}

void Viewer::init()
{
// 	//Restore previous viewer state.
// 	restoreStateFromFile();

// 	setMouseBinding(Qt::LeftButton, SELECT);
// 	setMouseBinding(Qt::LeftButton + Qt::CTRL, SELECT);
// 	setMouseBinding(Qt::LeftButton + Qt::ALT, SELECT);
// // 	setMouseBinding(Qt::LeftButton, NO_CLICK_ACTION, true);
// // 	setMouseBinding(Qt::LeftButton + Qt::CTRL, NO_CLICK_ACTION, true);
// // 	setMouseBinding(Qt::LeftButton + Qt::ALT, NO_CLICK_ACTION, true);
// 
// 	setMouseBinding(Qt::MidButton, CAMERA, ROTATE);
// 	//setMouseBinding(Qt::MidButton + Qt::CTRL, FRAME, ROTATE);
// 	setMouseBinding(Qt::MidButton, ALIGN_CAMERA, true);
// 	//setMouseBinding(Qt::MidButton + Qt::CTRL, ALIGN_FRAME, true);

	glClearColor(1.f, 1.f, 1.f, 1.f);
// 	glClearColor(1.f, 0.f, 1.f, 1.f);

	camera()->setType(qglviewer::Camera::ORTHOGRAPHIC);	
	setSceneRadius(51.785);
	showEntireScene();
}

void Viewer::draw()
{
	double r = templ.GetBoundingSphereRadius();

	glEnable(GL_LIGHTING);
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	
 	glTranslated(-templ.cog[0], -templ.cog[1], -templ.cog[2]);

	DrawDeformationGraph();
	DrawHandles(r*0.02);
	DrawGeodesicPath(r*draw_handles_ratio);	
	DrawCorrenpondence();

	DrawMesh();
}
void Viewer::drawWithNames()
{
	glTranslated(-templ.cog[0], -templ.cog[1], -templ.cog[2]);

	glDisable(GL_CULL_FACE);
	double r = templ.GetBoundingSphereRadius();
	for(size_t i = 0; i < handles.size(); i++)
	{
		glPushName(-2-i);
		glPushMatrix();
		glTranslatev(handles[i].second);
		DrawSphere(r*draw_handles_ratio);
		glPopMatrix();
		glPopName();
	}

	if(templ_deformed.n_faces() <= 0) return;

	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);

	glVertexPointer(3, GL_DOUBLE, 0, templ_deformed.points());	
	glNormalPointer(GL_DOUBLE, 0, templ_deformed.vertex_normals());

	HCCLMesh::ConstFaceIter fIt(templ_deformed.faces_begin()), fEnd(templ_deformed.faces_end());
	HCCLMesh::ConstFaceVertexIter fvIt;		
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);	
	for (; fIt!=fEnd; ++fIt)
	{
		glPushName(fIt.handle().idx());
		fvIt = templ_deformed.cfv_iter(fIt.handle());
		glBegin(GL_TRIANGLES);
		glArrayElement(fvIt.handle().idx());
		glArrayElement((++fvIt).handle().idx());
		glArrayElement((++fvIt).handle().idx());
		glEnd();
		glPopName();
	}

	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);
}

void Viewer::mousePressEvent(QMouseEvent* e)
{
	if ((e->button() == Qt::LeftButton) && onEmbededDeformation)
	{
		QGLViewer::mousePressEvent(e);
		int sel_facet = selectedName();
		if(sel_facet < -1 && e->modifiers() == Qt::NoModifier)
		{
			int n = -sel_facet-2;
			int cnt = 0;
			for_each(handles.begin(), handles.end(), [&cnt, n](Handle p)
			{
				p.first[0] = (n==cnt) ? 1 : 0;
				++cnt;
			});
		}
		mouse_prev = qglviewer::Vec(e->x(), e->y(), 0);
		btn_pressed = Qt::LeftButton;
	}
	else
	{
		QGLViewer::mousePressEvent(e);
	}
}
void Viewer::mouseMoveEvent(QMouseEvent *e)
{
	if(btn_pressed == Qt::LeftButton && onEmbededDeformation)
	{
		if(!handles.empty())
		{
			mouse_curr = qglviewer::Vec(e->x(), e->y(), 0);
			qglviewer::Vec mouse_prev_unproj = camera()->unprojectedCoordinatesOf(mouse_prev);
			qglviewer::Vec mouse_curr_unproj = camera()->unprojectedCoordinatesOf(mouse_curr);
			qglviewer::Vec displacement = mouse_curr_unproj - mouse_prev_unproj;

			for_each(handles.begin(), handles.end(), [displacement](Handle& p)
			{
				if(p.first[0]) p.second += Vector3d(displacement.x, displacement.y, displacement.z);				
			});
			RunOptimization();
			onDrag = true;
			mouse_prev = mouse_curr;
			updateGL();
		}
	}
	QGLViewer::mouseMoveEvent(e);
}
void Viewer::mouseReleaseEvent(QMouseEvent *e)
{
	if ((e->button() == Qt::LeftButton) && !onDrag && onEmbededDeformation)
	{
		QGLViewer::mousePressEvent(e);
		int sel_facet = selectedName();

		if(sel_facet == -1) // select nothing
		{
			if(e->modifiers() == Qt::NoModifier)
				handles.clear();
		}
		else if(sel_facet >= 0 && sel_facet < templ_deformed.n_faces()) // select facet
		{
			if(e->modifiers() != Qt::CTRL) handles.clear();

			HCCLMesh::ConstFaceVertexIter fvit = templ_deformed.cfv_iter(templ_deformed.face_handle(sel_facet));

			int min_idx = 0;
			double min_val = std::numeric_limits<double>::infinity();
			HCCLMesh::Point pt;
			qglviewer::Vec pt_;
			qglviewer::Vec s;

			pt = templ_deformed.point(fvit);
			pt_[0] = pt[0];		pt_[1] = pt[1];		pt_[2] = pt[2];
			s = camera()->projectedCoordinatesOf(pt_);
			min_val = (s[0] - e->x())*(s[0] - e->x()) + (s[1] - e->y())*(s[1] - e->y());
			min_idx = fvit.handle().idx();

			pt = templ_deformed.point(++fvit);
			pt_[0] = pt[0];		pt_[1] = pt[1];		pt_[2] = pt[2];
			s = camera()->projectedCoordinatesOf(pt_);
			if(min_val > (s[0] - e->x())*(s[0] - e->x()) + (s[1] - e->y())*(s[1] - e->y()) )
			{
				min_val = (s[0] - e->x())*(s[0] - e->x()) + (s[1] - e->y())*(s[1] - e->y());
				min_idx = fvit.handle().idx();
			}

			pt = templ_deformed.point(++fvit);
			pt_[0] = pt[0];		pt_[1] = pt[1];		pt_[2] = pt[2];
			s = camera()->projectedCoordinatesOf(pt_);
			if(min_val > (s[0] - e->x())*(s[0] - e->x()) + (s[1] - e->y())*(s[1] - e->y()) )
			{
				min_val = (s[0] - e->x())*(s[0] - e->x()) + (s[1] - e->y())*(s[1] - e->y());
				min_idx = fvit.handle().idx();
			}

			handles.push_back(Handle(Vector3d(0,min_idx,0), cast_to_Vector3d(templ_deformed.point(templ_deformed.vertex_handle(min_idx)))));

		}
		else if(sel_facet < -1)
		{
			int n = -sel_facet-2;
			if (e->modifiers() == Qt::ALT)
				handles.erase(handles.begin() + n);
			else if (e->modifiers() == Qt::CTRL)
				handles[n].first[0] = handles[n].first[0] ? 0 : 1;
			else if(e->modifiers() == Qt::NoModifier)
				for_each(handles.begin(), handles.end(), [](Handle p){p.first[0] = 0;});
		}
		updateGL();
	}

	onDrag = false;
	btn_pressed = Qt::NoButton;
	QGLViewer::mouseReleaseEvent(e);
}

/////////////////
// Summner part//
/////////////////
void Viewer::InitOptimization()
{
	// building graph
	graph.SetMesh(&templ);
	graph.BuildGraph(min(300.0/templ.n_vertices(), 1.0), 2);

	// the number of nodes which influence each vertex
	nearest_k = 4;
	weight_value.resize(templ.n_vertices(), vector<double>(nearest_k+1));
	k_nearest_idx.resize(templ.n_vertices(), vector<int>(nearest_k+1));

	// get weight values of each vertex
	int start_tic = clock();

	for (int i = 0; i<weight_value.size(); ++i)
	{		
		Vector3d vertex(templ.point(templ.vertex_handle(i))[0], templ.point(templ.vertex_handle(i))[1], templ.point(templ.vertex_handle(i))[2]);
		graph.FindClosestPoint(vertex , k_nearest_idx[i].data(), nearest_k+1);
		
		for (int j = 0; j<nearest_k+1; ++j)
			weight_value[i][j] = Norm(vertex - graph.nodes[k_nearest_idx[i][j]]);

		double sum = 0;
		for (int j = 0; j<nearest_k; ++j)
		{
			double temp = 1-weight_value[i][j]/weight_value[i][nearest_k];
			weight_value[i][j] = temp*temp;
			sum += weight_value[i][j];
		}
		for (int j = 0; j<nearest_k; ++j)
			weight_value[i][j] /= sum;
	}
	graph.draw_nodes = graph.nodes;

	printf("set k-nearest node and weight values : %f sec\n", (clock()-start_tic)/double(CLOCKS_PER_SEC));	
}
void Viewer::RunOptimization()
{
	int start_tic = clock();
	int n_node = graph.nodes.size();

	// Init LBFGS
	epsg = 0.0001;
	epsf = 0.0001; // 10^-9
	epsx = 0.0001;
	maxits = 200;
	x.setbounds(1, n_node*12);
	nbd.setbounds(1, n_node*12);
	lbd.setbounds(1, n_node*12);
	ubd.setbounds(1, n_node*12);
	for(int i = 1; i <= n_node*12; ++i)
	{
		x(i) = (i%12 == 1 || i%12 == 5 || i%12 == 9) ? 1 : 0;
		nbd(i) = 0;
		lbd(i) = 0.0;
		ubd(i) = 0.0;
	}

	// run LBFGS
	int info = 0;
	lbfgsbminimize(n_node*12, 5, x, graph, templ, target, k_nearest_idx, weight_value, alph, handles, correnpondence, this, epsg, epsf, epsx, maxits, nbd, lbd, ubd, info);

	// update solution
 	Deform(templ, x, templ_deformed, graph);

	printf("time for one iteration : %f sec\n", (clock()-start_tic)/double(CLOCKS_PER_SEC));
}
void Viewer::Deform( const CTriMesh& ori, const ap::real_1d_array& defrom_result, CTriMesh& mesh, DeformationGraph& dgraph )
{
	int n_node = graph.nodes.size();

	vector<Vector3d> result_translation(n_node);
	vector<std::vector<double>> result_rotation(n_node, vector<double>(9));

	for(int i = 0; i < n_node; ++i)
	{
		result_translation[i] = Vector3d(defrom_result(12*i+10), defrom_result(12*i+11), defrom_result(12*i+12));
		for (int j = 0; j < 9; ++j)
			result_rotation[i][j] = defrom_result(12*i+j+1);
	}

	HCCLMesh::ConstVertexIter cvit = ori.vertices_begin();
	HCCLMesh::VertexIter vit = mesh.vertices_begin();

	// mesh update
	for (; cvit!=ori.vertices_end(); ++cvit, ++vit)
	{
		int idx_vrtx = cvit.handle().idx();

		arma::vec  v_ = arma::vec().zeros(3);		
		arma::vec  v(3);
		HCCLMesh::Point tem = ori.point(cvit);
		v[0] = tem[0];
		v[1] = tem[1];
		v[2] = tem[2];

		for (int j = 0; j<nearest_k; ++j)
		{
			int idx_node = k_nearest_idx[idx_vrtx][j];

			double w = weight_value[idx_vrtx][j];
			arma::vec g(dgraph.nodes[idx_node].val, 3);
			g[0] = dgraph.nodes[idx_node][0];
			g[1] = dgraph.nodes[idx_node][1];
			g[2] = dgraph.nodes[idx_node][2];

			arma::vec t(result_translation[idx_node].val, 3);
			t[0] = result_translation[idx_node][0];
			t[1] = result_translation[idx_node][1];
			t[2] = result_translation[idx_node][2];

			arma::mat R(result_rotation[idx_node].data(), 3, 3);

			arma::vec d = v-g;
			arma::vec rd;

			rd = R*d;		
			v_ += w*(rd+g+t);
		}

		mesh.set_point(*vit, HCCLMesh::Point(v_[0], v_[1], v_[2]));
	}

	mesh.update_normals();

	// deformation graph update
	std::transform(graph.nodes.begin(), graph.nodes.end(), result_translation.begin(), dgraph.draw_nodes.begin(), [](const Vector3d& a, const Vector3d& b){return a+b;});

	updateGL();
}

/////////////////
// Hoa Li part //
/////////////////
void Viewer::InitOptimization_HaoLi()
{	
	graph.Clear();
	graph.SetMesh(&templ);
// 	graph.BuildGraph(0.3, 2);
	graph.BuildGraph(std::min(300.0/templ.n_vertices(), 1.0), 2);

	int n_node = graph.NumNodes();

	// the number of nodes which influence each vertex
	nearest_k = 3;
	weight_value.resize(templ.n_vertices(), vector<double>(nearest_k));
	k_nearest_idx.resize(templ.n_vertices(), vector<int>(nearest_k));

	// get weight values of each vertex	
	cout << "calculate weight values of each vertex..";
	for (int i = 0; i<weight_value.size(); ++i)
	{
		vector<int> tem_indx(nearest_k+1);
		Vector3d vertex = cast_to_Vector3d(templ.point(templ.vertex_handle(i)));
		graph.FindClosestPoint(vertex , tem_indx.data(), nearest_k+1);
		
		double r_squared = NormSquared(vertex - graph.nodes[tem_indx[nearest_k]]);

		double sum = 0;
		for (int j = 0; j<nearest_k; ++j)
		{
			double tem = 1 - NormSquared(vertex - graph.nodes[tem_indx[j]])/r_squared;
			weight_value[i][j] = max(0.0, tem*tem*tem);
			sum += weight_value[i][j];

			k_nearest_idx[i][j] = tem_indx[j];
		}

		for (int j = 0; j<nearest_k; ++j)
			weight_value[i][j] /= sum;
	}
	cout << "finish!" << endl;

	graph.draw_nodes = graph.nodes;
	
	// Init LBFGS
	epsg = 0.0;
	epsf = 0.000000001; // 10^-9
	epsx = 0.0;
	maxits = 1000;	
	
	x.setbounds(1, n_node*12);
	nbd.setbounds(1, n_node*12);
	lbd.setbounds(1, n_node*12);
	ubd.setbounds(1, n_node*12);

	for(int i = 1; i <= n_node*12; ++i)
	{
		x(i) = (i%12 == 1 || i%12 == 5 || i%12 == 9) ? 1 : 0;
			nbd(i) = 0;
			lbd(i) = 0.0;
			ubd(i) = 0.0;
	}

	// initial correspondence
	correnpondence.clear();
	correnpondence.reserve(graph.nodes.size());
	target.BuildKDTree();
	for (int i = 0; i<graph.nodes.size(); ++i)
	{
		Vector3d template_point = graph.nodes[i];
		Vector3d template_normal = cast_to_Vector3d(templ.vertex_normals()[graph.node_indxes[i]]);

		Vector3d target_point;
		int target_indx;
		target.FindClosestPoint(template_point, &target_indx, 1, &target_point);
		Vector3d target_normal = cast_to_Vector3d(target.vertex_normals()[target_indx]);		

		// discard correspondence
		double distance_threshold = alph[4];
		double normal_threshold = alph[5];
		if ((template_point-target_point).Norm()>distance_threshold || template_normal.Dot(target_normal) < 0.6/* || target.is_boundary(target.vertex_handle(target_indx))*/) continue;

		correnpondence.push_back(pair<int, int>(i, target_indx));
	}

	templ_deformed.Clear();
	templ_deformed = templ;
	templ_deformed.SetTransparency(0.7);
	onHaoli = true;
}
void Viewer::RunOptimization_HaoLi()
{	
	cout << "start!!" << endl<<endl<<endl;

	int info = 0;
	lbfgsbminimize(graph.NumNodes()*12, 5, x, graph, templ, target, k_nearest_idx, weight_value, alph, handles, correnpondence, this, epsg, epsf, epsx, maxits, nbd, lbd, ubd, info);

	// update
	Deform(templ, x, templ_deformed, graph);
	updateGL();
	//onHaoli = false;
}

// geodesic path
void Viewer::InitGeo()
{
	onGeodesic = false;
	templ.ClearGeo();
	templ.InitGeo();

	source_vertex_index.clear();
	source_vertex_index.resize(1);
	source_vertex_index[0] = 0;
// 	source_vertex_index[1] = 200;
// 	source_vertex_index[2] = 800;

	target_vertex_index.clear();
	target_vertex_index.resize(1);
	target_vertex_index[0] = 800;
// 	target_vertex_index[1] = 700;
// 	target_vertex_index[2] = 500;
// 	target_vertex_index[3] = 100;
// 	target_vertex_index[4] = 60;
}
void Viewer::Propagate()
{
	onGeodesic = true;
	templ.Propagate(source_vertex_index);
	templ.GetGeodesicDistanceAll();
	templ.SetVertexColor2GeodesicDist();

	//double g_length = templ.GetGeodesicPath(target_vertex_index[0]);
	std::vector<double> g_length = templ.GetGeodesicPath(target_vertex_index);

	updateGL();
}

// draw functions
void Viewer::DrawMesh()
{
	// Draw Template Mesh
	if(pParentDlg->ui.actionTemplateVisible->isChecked())
	{
		glLineWidth(0.5);
		
		if (onEmbededDeformation || onHaoli)
			templ_deformed.Render(pParentDlg->ui.actionSmooth->isChecked());
		else
			templ.Render(pParentDlg->ui.actionSmooth->isChecked(), onGeodesic);

//		// boundary points
// 		glColor4ub(60,120,60,255);
// 		HCCLMesh::VertexIter vit(templ.vertices_begin()), vend(templ.vertices_end());
// 		for (; vit!=vend; ++vit)
// 		{
// 			if (templ.is_boundary(vit))
// 			{
// 				glPushMatrix();
// 				glTranslatev(cast_to_Vector3d(templ.point(vit)));			
// 				DrawSphere(r*0.001);
// 				glPopMatrix();
// 			}
// 		}
	}

	// Draw Target Mesh
	if(pParentDlg->ui.actionTargetVisible->isChecked())
	{
		glLineWidth(1.0);
		if(onEmbededDeformation)
			target.SetRenderColor(255, 190, 100);
		else
			target.SetRenderColor(100, 190, 255);
		target.Render(pParentDlg->ui.actionSmooth_2->isChecked());
	}

}
void Viewer::DrawDeformationGraph()
{
	// Draw Deformation graph
	if(pParentDlg->ui.actionDeformationGraphVisible->isChecked())
	{
		glDisable(GL_LIGHTING);
		glHint(GL_POINT_SMOOTH_HINT, GL_NICEST );
		glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
		glEnable(GL_POINT_SMOOTH);
		glEnable(GL_LINE_SMOOTH);

		// 		glColor3ub(0,0,0);
		// 		glPointSize(10.0);
		// 		glLineWidth(2.0);
		// 		glEnable(GL_DEPTH_TEST);
		// 		graph.Render();

		// 		glColor4ub(60,120,60,255);
		// 		for(int i = 0; i < graph.draw_nodes.size(); i++)
		// 		{
		// 			glPushMatrix();
		// 			glTranslatev(graph.draw_nodes[i].val);			
		// 			DrawSphere(r*0.005);
		// 			glPopMatrix();
		// 		}

		glColor3ub(227,9,74);
		glLineWidth(3.0);
		glBegin(GL_LINES);
		for(int i = 0; i < graph.edges.size(); i++)
		{
			glVertex3dv(graph.draw_nodes[graph.edges[i][0]].val);
			glVertex3dv(graph.draw_nodes[graph.edges[i][1]].val);
		}
		glEnd();

		glDisable(GL_POINT_SMOOTH);
		glDisable(GL_LINE_SMOOTH);
		glEnable(GL_LIGHTING);
	}
}
void Viewer::DrawHandles( double r )
{
	// Draw selected points
// 	for(size_t i = 0; i < selected_vertex_idx.size(); i++)
// 	{
// 		glPushMatrix();
// 		glTranslatev(moved_point[i]);
// 		if(std::find(selected_handle_idx.begin(), selected_handle_idx.end(), i) != selected_handle_idx.end())
// 			glColor4ub(255,100,190,200);
// 		else
// 			glColor4ub(100,190,255,200);
// 		DrawSphere(r);
// 		glPopMatrix();
// 	}

	for(size_t i = 0; i < handles.size(); i++)
	{
		glPushMatrix();
		glTranslatev(handles[i].second);
		if(handles[i].first[0])
			glColor4ub(255,100,190,200);
		else
			glColor4ub(100,190,255,200);
		DrawSphere(r);
		glPopMatrix();
	}


}
void Viewer::DrawGeodesicPath(double r)
{
	// Draw Geodesic path
	if(onGeodesic)
	{
		// source vertex
		for (int i = 0; i<source_vertex_index.size(); ++i)
		{
			glColor4ub(255,100,190,200);
			glPushMatrix();
			glTranslatev(templ.point(templ.vertex_handle(source_vertex_index[i])).data());
			DrawSphere(r);
			glPopMatrix();
		}

		// target vertex
		for (int i = 0; i<target_vertex_index.size(); ++i)
		{
			glColor4ub(100,190,255,200);
			glPushMatrix();
			glTranslatev(templ.point(templ.vertex_handle(target_vertex_index[i])).data());
			DrawSphere(r);
			glPopMatrix();
		}

		// geodesic path
		glColor3ub(255,0,0);
		for (int i = 0; i<templ.geo_path.size(); ++i)
		{
			// 			glEnable(GL_LINE_SMOOTH);
			// 			glDisable(GL_LIGHTING);
			// 			glLineWidth(5.0);
			// 			glBegin(GL_LINE_STRIP);
			for (int j = 0; j<templ.geo_path[i].size(); ++j)
			{
				// 				glVertex3dv(templ.geo_path[i][j].xyz());
				glPushMatrix();
				glTranslatev(templ.geo_path[i][j].xyz());
				DrawSphere(0.25*r);
				glPopMatrix();
			}
			// 			glEnd();
			// 			glDisable(GL_LINE_SMOOTH);
			// 			glLineWidth(1.0);
			// 			glEnable(GL_LIGHTING);

		}
	}

}
void Viewer::DrawCorrenpondence()
{
	// Draw corresponding points
	if (onHaoli)
	{
		glDisable(GL_LIGHTING);

		glEnable(GL_POINT_SMOOTH);		
		glPointSize(5.0);
		glColor4ub(180,30,150,200);
		glBegin(GL_POINTS);
		for (int i = 0; i<correnpondence.size(); ++i)
			glVertex3dv(target.points()[correnpondence[i].second].data());
		glEnd();
		glDisable(GL_POINT_SMOOTH);

		glEnable(GL_LINE_SMOOTH);
		glLineWidth(2.0);
		glColor4ub(0,200,0,255);
		glBegin(GL_LINES);
		for (int i = 0; i<correnpondence.size(); ++i)
		{
			glVertex3dv(graph.draw_nodes[correnpondence[i].first].val);
			glVertex3dv(target.points()[correnpondence[i].second].data());
		}
		glEnd();
		glDisable(GL_LINE_SMOOTH);


		glEnable(GL_LIGHTING);
	}
}

void Viewer::LoadShader( QString vshader_, QString fshader_ )
{
	CleanShader();
	shader_program = new QGLShaderProgram(this);

	QGLShader *vsh = new QGLShader(QGLShader::Vertex, this);
	const char *vsrc =
		"void main(void)\n"
		"{\n"
		"    gl_Position = matrix * vertex;\n"
		"}\n";
	if(vsh->compileSourceCode(vsrc)) shader_program->addShader(vsh);
	else qWarning() << "Vertex Shader Error" << vsh->log();

	QGLShader *fsh = new QGLShader(QGLShader::Fragment, this);
	const char *fsrc =
		"void main(void)\n"
		"{\n"
		"    gl_FragColor = vec4(0.4,0.4,0.8,1.0);\n"
		"}\n";
	if(fsh->compileSourceCode(fsrc)) shader_program->addShader(fsh);
	else qWarning() << "Fragment Shader Error" << fsh->log();


// 	// load and compile vertex shader
// 	QFileInfo vsh(vshader);
// 	if(vsh.exists())
// 	{
// 		VertexShader = new QGLShader(QGLShader::Vertex);
// 		if(VertexShader->compileSourceFile(vshader))
// 			shader_program->addShader(VertexShader);
// 		else qWarning() << "Vertex Shader Error" << VertexShader->log();
// 	}
// 	else qWarning() << "Vertex Shader source file " << vshader << " not found.";
// 
// 	// load and compile fragment shader
// 	QFileInfo fsh(fshader);
// 	if(fsh.exists())
// 	{
// 		FragmentShader = new QGLShader(QGLShader::Fragment);
// 		if(FragmentShader->compileSourceFile(fshader))
// 			shader_program->addShader(FragmentShader);
// 		else qWarning() << "Fragment Shader Error" << FragmentShader->log();
// 	}
// 	else qWarning() << "Fragment Shader source file " << fshader << " not found.";

	if(!shader_program->link())
	{
		qWarning() << "Shader Program Linker Error" << shader_program->log();
	}
	else shader_program->bind();
}

void Viewer::CleanShader()
{
	if(shader_program)
	{
		shader_program->release();
		shader_program->removeAllShaders();
		shader_program = NULL;
	}

	if(VertexShader)
	{
		delete VertexShader;
		VertexShader = NULL;
	}

	if(FragmentShader)
	{
		delete FragmentShader;
		FragmentShader = NULL;
	}

}
